#include	"catmullCurve.h"

#define NBCHAR 800

catmullCurve::catmullCurve()
{
	currenttime = 0.0;
	displist = 0;
}

catmullCurve::~catmullCurve()
{
	
}

bool catmullCurve::parse(const char *filename)
{
	 FILE *m_FilePointer = fopen(filename, "r");
	 if(!m_FilePointer)
	{
		cout <<  "Unable to find or open the file: " << filename << endl ;
		return false;
	}
	char strLine[255] = {0} ;
	controlPoint cp;
	cout << "Loading path file " << filename << endl ;
	
	while(!feof(m_FilePointer))
	{
		fscanf(m_FilePointer, "t=%f : x=%f , y=%f , z=%f", &cp.time, &cp.pos.x, &cp.pos.y, &cp.pos.z);
//		 cout << cp.time << " " << cp.pos.x << " " << cp.pos.y << " " << cp.pos.z << endl ;
		fgets(strLine,NBCHAR,m_FilePointer);
		cpoints.push_back(cp);
	}
	fclose(m_FilePointer);
	
	rescaleTime();
	return true;
	
}

void catmullCurve::rescaleTime()
{
	totalTime = cpoints[cpoints.size()-1].time ;
	float currentTime = 0.0 ;
	float totaldist = 0.0 ;

	// compute the total length of the control segments
	for (unsigned int i =1 ; i<cpoints.size() ;i++)
	{
		totaldist += (cpoints[i].pos - cpoints[i-1].pos).Norm() ;
	}

	// linearize time to obtain a constant speed
	for (unsigned int i =1 ; i<cpoints.size() ;i++)
	{
		float dist = (cpoints[i].pos - cpoints[i-1].pos).Norm() ;
		currentTime +=  totalTime * (dist/totaldist) ;
		cpoints[i].time = currentTime ;
	}
}

// find the time span around t
unsigned int catmullCurve::findspan(float t)
{
	int spanup = cpoints.size()-1 ;
	int spandown = 0 ;
	while (spanup!=spandown+1)
	{
		if (cpoints[spanup-1].time>t) spanup-=1 ;
		if (cpoints[spandown+1].time<t) spandown+=1 ;
	}
	return spandown ;
}

void catmullCurve::evalpos(float t)
{
	currenttime += t;
	if(currenttime > totalTime ) currenttime = 0.0;
	float timetemp = currenttime ;
	
	unsigned int s = findspan(currenttime);
	int p1,p2,p3,p4 ;

	// the indexes of the 4 control points
	p1 = s-1 ;
	p2 = s ;
	p3 = s+1 ;
	p4 = s+2 ;

	// if we are on the first segment of the curve
	if(s==0) 
	{ 
		p1=p2=0; 
		p3=1; 
		p4=2; 
	}

	// if we are on the last segment of the curve
	if(s==cpoints.size()-2) 
	{ 
		p1=s-1; 
		p2=s; 
		p3=p4=s+1; 
	}

	// if we are right on a control point
	if(timetemp==cpoints[s].time) 
	{ 
		currentpos = cpoints[s].pos;
	}

	// we rescale the time so that it always is between 0 and 1
	timetemp = (timetemp-cpoints[s].time)/(cpoints[s+1].time - cpoints[s].time);

	currentpos = 0.5 * ((-cpoints[p1].pos + 3.0*cpoints[p2].pos -3.0*cpoints[p3].pos + cpoints[p4].pos)*timetemp*timetemp*timetemp
					+ (2.0*cpoints[p1].pos -5.0*cpoints[p2].pos + 4.0*cpoints[p3].pos - cpoints[p4].pos)*timetemp*timetemp
					+ (-cpoints[p1].pos+cpoints[p3].pos)*timetemp
					+ 2.0*cpoints[p2].pos) ;

}

Vec3 catmullCurve::animate(float t)
{
	evalpos(t);
	return currentpos ;
}

Vec3 catmullCurve::getCurrentPos()
{
	return currentpos;
}

void catmullCurve::draw()
{
	if(!displist)
	{
		displist = glGenLists(1);
		glPointSize(3);
		
		glNewList(displist,GL_COMPILE);
			glBegin(GL_LINE_STRIP);
				static int nbsubdiv = 100;
				for(int i=0;i<nbsubdiv-1;i++)
				{
					float increment = totalTime/nbsubdiv;
					Vec3 pos = animate(increment);
					glVertex3f(pos.x,pos.y,pos.z);
				}
			glEnd();
			glBegin(GL_POINTS);
				for(unsigned int i=0;i<cpoints.size();i++)
				{
					glVertex3f(cpoints[i].pos.x,cpoints[i].pos.y,cpoints[i].pos.z);
				}
			glEnd();
		glEndList();
		
		currenttime=0.0;
	}
	else  glCallList(displist) ;
}




